#include "udp_gps_forwarder/UavUdpComm.hpp"
#include "rclcpp/rclcpp.hpp"
#include <pthread.h>

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);

    pthread_t pt1,pt2,pt3;
    void *tret1,*tret2,*tret3;
    pthread_create(&pt1,NULL,setOffboardMode,NULL);//drone_0
    pthread_create(&pt2,NULL,set1ffboardMode,NULL);//drone_1
    pthread_create(&pt3,NULL,set2ffboardMode,NULL);//drone_2

    auto node = std::make_shared<UavUdpComm>();
    rclcpp::spin(node);
    rclcpp::shutdown();

    pthread_join(pt1,&tret1);
    pthread_join(pt2,&tret2);
    pthread_join(pt3,&tret3);
    return 0;
}
